

#ifndef __CCPP_PATH_PROCESS_H__
#define __CCPP_PATH_PROCESS_H__

#include "common/cgal_def.h"
#include "polygon2d.h"

#include <opencv2/opencv.hpp>

namespace global_path_planner {
namespace ccpp {

class CCPPPathProcess{
public:
    explicit CCPPPathProcess(std::shared_ptr<Polygon2D>& poly2d);
    virtual ~CCPPPathProcess();
    
    // 根据当前点重排下个清扫区域的路径
    void sortNextSweepPath(const Point_2& curr_point, std::vector<Point_2>& next_sweep);

    // 移除单个cell中重复的路径
    void removeCellPathsLessSweepRadius(std::vector<Point_2>& cell_paths, float sweep_radius_2);

    // 移除相邻的路径小于路径宽度的一半
    void removeRepeatPathsLessSweepRadius(std::vector<std::vector<Point_2>>& cells_sweeps, float sweep_radius_2);

    // 重新cell的索引
    std::deque<int> sortCellsIndex(std::vector<std::vector<Point_2>>& cells_sweeps, Point_2 &start);

    // 当前点离下个规划区域最短的距离
    double getCellSweepMinDist(const Point_2& curr_point, 
                            std::vector<Point_2>& next_sweep, 
                            std::vector<Point_2>::iterator& iter);

    // 根据当前点对下一个清扫cell重排序
    void doReverseNextSweep(const Point_2& curr_point, std::vector<Point_2>& next_sweep);

    // 移除路径在障碍物中
    std::vector<std::vector<Point_2>> removeObstacleSweepPaths(cv::Mat& img, std::vector<std::vector<Point_2>> &all_sweep_paths);
    
    // 根据当前点调整回字形路径
    std::vector<std::pair<float, float>> sortLoopPath(std::pair<float, float>& start, 
                                        std::vector<std::pair<float, float>> loop_path);

private:
    //删除点到直线的距离小于清扫半径的点集
    void removePointToLineLessPathWidth(const float& path_width_2, 
                                    std::vector<Point_2>& cur_cell_sweep, 
                                    std::vector<Point_2>& adj_cell_sweep);

    // 使用二分法获取线段中一点到另一个线段的距离等于规划路径宽度的一半
    Point_2 getPointOfLine(Segment_2& line, Point_2& p1, 
                        Point_2& p2, float sweep_radius_2);

    // 更新点到直线的距离小于路径宽度的一半
    bool pointToLineLessSweepRadius2(Segment_2& seg, float sweep_radius_2,
                                    std::vector<Point_2>& cell_paths,
                                    std::vector<Point_2>::iterator& iter);

    // 获取一条直线距离另一条直线的最小端点
    std::pair<double, std::vector<Point_2>::iterator>
    getPointOfMinDis(Segment_2& cur_seg, Segment_2& next_seg,
                    std::vector<Point_2>::iterator& cur_it,
                    std::vector<Point_2>::iterator& next_it);

private:
    std::shared_ptr<Polygon2D> __polygon2d_Ptr;

};

}  // namespace ccpp
}  // namespace global_path_planner

#endif  // __CCPP_PATH_PROCESS_H__